/****************************************************************************/
/*	include files
*****************************************************************************/
#include "include.h"

volatile APPUSER_T AppUser = {0};
volatile CPCSTATES_T CPQ_STATE = RUN_INIT;
volatile CPQAPP_T CPQUser = {0};
volatile u8 TopBlockCause = 0;
volatile u8 BottomBlockCause = 0;


#define POS2PRESSCnt			1250//1300

void AppUserInitFun(void)
{
	AppUser.TargetRunCnt = 200000;//156250;//200000;
	AppUser.MotorRunTimeCnt = 0;
	AppUser.MotorRestTime = 0;
}

void CPQAPPInitFun(void)
{
	CPQ_STATE = RUN_INIT;
	CPQUser.NEGRunCnt = 0;
	CPQUser.POSRunCnt = 0;
	CPQUser.BREAKRunCnt = 0;
	CPQUser.HALLRunCnt = 0;
	CPQUser.SpeedLowCnt = 0;
	CPQUser.PourFlag = 0;
	CPQUser.PressFlag = 0;
	CPQUser.PressStableCurrent = 0;

	CPQUser.PressDeltaCurrent = 0;
	CPQUser.PourStableCurrent = 0;
	CPQUser.PourDeltaCurrent = 0;
	CPQUser.BottomBlockCnt = 2048;
	CPQUser.TopBlockCnt = 2048;

	CPQUser.BottomCurrentLevel = CURRENT_6A;
	CPQUser.PressCurrentLevel = 0;//PRESS_40KG;
	CPQUser.PressFinshFlag = 0;
	CPQUser.PourFinshFlag = 0;
	TopBlockCause = 0;
}



void CPQAPPResetFun(void)
{
	CPQ_STATE = RUN_INIT;
	CPQUser.FilterCnt = 0;
	CPQUser.NEGRunCnt = 0;
	CPQUser.POSRunCnt = 0;
	CPQUser.BREAKRunCnt = 0;
	CPQUser.HALLRunCnt = 0;
	CPQUser.SpeedLowCnt = 0;
	CPQUser.PourStableCurrent = 0;
	CPQUser.PourDeltaCurrent = 0;
	CPQUser.CPQHALLACnt = 0;
	CPQUser.CPQHALLBCnt = 0;
	CPQUser.CPQHALLCCnt = 0;
}

/******3s = 3000ms;3000*20 = 60000**************/
void MotorRunTimeControlFun(u32 TimeVal)
{
	if(((MOTOR_STATE == MC_SQUARE)||(MOTOR_STATE == MC_FOC))&&(PROTECT_CAUSE == PROTECT_NONE))
	{
		AppUser.MotorRunTimeCnt++;
		AppUser.MotorRestCnt = TimeVal - AppUser.MotorRunTimeCnt;
		if(AppUser.MotorRestCnt < 0)AppUser.MotorRestCnt = 0;
		if(AppUser.MotorRunTimeCnt > TimeVal)
		{
			CPQUser.MotorEnable = 0;
			AppUser.MotorRunTimeCnt = 0;
			AppUser.MotorRestTime = 0;
			StartValue = 0;
		}
	}
}








void CPQNEGRunFun(void)
{
	if(CPQ_STATE == RUN_NEG)
	{
		if((++CPQUser.NEGRunCnt >= T1SCNT)||(CPQUser.HALLRunCnt >= 300))
//	if(CPQUser.HALLRunCnt >= 3200)
		{
			CPQUser.NEGRunCnt = 0;
			CPQ_STATE = RUN_POUR;
			CPQUser.PourStableCurrent = Foc_lib.IqFilter;//save iqFilter
		}
	}
}



void CPQPOSRunFun(void)
{
//3478>(2000+1360)	
	if(CPQ_STATE == RUN_POS)
	{
		if(CPQUser.PourFlag == False)
		{
			if((CPQUser.HALLRunCnt >= POS2PRESSCnt))//1200;1000;900
			{
				CPQUser.POSRunCnt = 0;
				CPQUser.PressStableCurrent = Foc_lib.IqFilter;
				CPQ_STATE = RUN_PRESS;
				Foc_lib.TargetIq = CPQUser.PressStableCurrent + CPQUser.PressCurrentLevel;
			}
		}
	}
}


void CPQStopScanFun(void)
{
	CPQ_STATE = RUN_INIT;
	MOTOR_STATE = MC_STOP;
	CPQUser.MotorEnable = 0;
	StartValue = 0;
}


void CPQBottomBlockScanFun(void)
{
	if(++CPQUser.CPQHALLACnt >= CPQUser.BottomBlockCnt)//3125
	{
		CPQUser.CPQHALLACnt = 0;
		CPQ_STATE = RUN_BREAK_BOTTOM;
		CPQUser.PourFlag = True;
		MOTOR_STATE = MC_BREAK;
		BottomBlockCause = 1;
	}
	if(++CPQUser.CPQHALLBCnt >= CPQUser.BottomBlockCnt)
	{
		CPQUser.CPQHALLBCnt = 0;
		CPQ_STATE = RUN_BREAK_BOTTOM;
		CPQUser.PourFlag = True;
		MOTOR_STATE = MC_BREAK;
		BottomBlockCause = 1;
	}
	if(++CPQUser.CPQHALLCCnt >= CPQUser.BottomBlockCnt)
	{
		CPQUser.CPQHALLCCnt = 0;
		CPQ_STATE = RUN_BREAK_BOTTOM;
		CPQUser.PourFlag = True;
		MOTOR_STATE = MC_BREAK;
		BottomBlockCause = 1;
	}
}


void CPQNEGRunCurrentScanFun(void)
{
	if(Foc_lib.IqFilter > Foc_lib.PourBusCurrentLevel)
	{
		CPQ_STATE = RUN_BREAK_BOTTOM;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PourFlag = True;
		BottomBlockCause = 3;
	}
}	




void CPQTopSpeedScanFun(void)
{
	if((Hall.HallOmegaPeroid < Protect.BlockSpeedL)||(Hall.HallOmegaPeroid  > Protect.BlockSpeedH))
	{
		if(++CPQUser.SpeedLowCnt >= 8)
		{
			CPQUser.SpeedLowCnt = 0;
			CPQ_STATE = RUN_BREAK_TOP;
			MOTOR_STATE = MC_BREAK;
			CPQUser.MotorEnable = 0;
			StartValue = 0;
			TopBlockCause = 3;
		}
	}
	else 
	{
		if(CPQUser.SpeedLowCnt > 0)CPQUser.SpeedLowCnt--;
	}
}


void CPQTopHallBlockScanFun(void)
{
	if(++CPQUser.CPQHALLACnt >= CPQUser.TopBlockCnt)//CPQUser.TopBlockCnt
	{
		CPQUser.CPQHALLACnt = 0;
		CPQ_STATE = RUN_BREAK_TOP;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PressFlag = True;
		TopBlockCause = 2;
	}
	if(++CPQUser.CPQHALLBCnt >= CPQUser.TopBlockCnt)
	{
		CPQUser.CPQHALLBCnt = 0;
		CPQ_STATE = RUN_BREAK_TOP;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PressFlag = True;
		TopBlockCause = 2;
	}
	if(++CPQUser.CPQHALLCCnt >= CPQUser.TopBlockCnt)
	{
		CPQUser.CPQHALLBCnt = 0;
		CPQ_STATE = RUN_BREAK_TOP;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PressFlag = True;
		TopBlockCause = 2;
	}
}


void CPQPourCurrentScanFun(void)
{
	CPQUser.PourDeltaCurrent = Foc_lib.IqPress - CPQUser.PourStableCurrent;
	if(CPQUser.PourDeltaCurrent >= CPQUser.BottomCurrentLevel)
	{
		CPQUser.PourDeltaCurrent = 0;
		CPQ_STATE = RUN_BREAK_BOTTOM;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PourFlag = True;
		BottomBlockCause = 2;
	}
}

void CPQPourScanFun(void)
{	
	CPQPourCurrentScanFun();
	CPQBottomBlockScanFun();
}

//CPQUser.HALLRunCnt all cnt 1710
void CPQPressCurrentScanFun(void)
{
	CPQUser.PressDeltaCurrent = Foc_lib.IqPress - CPQUser.PressStableCurrent;

	if((CPQUser.HALLRunCnt >= (POS2PRESSCnt+100)))
	{
		if(CPQUser.PressDeltaCurrent >= CPQUser.PressCurrentLevel)//1450;1300
		{
			if(++CPQUser.FilterCnt >= 1)
			{
				CPQUser.FilterCnt = 0;
				CPQUser.PressDeltaCurrent = 0;
				Foc_lib.IqPress = 0;
				CPQ_STATE = RUN_BREAK_TOP;
				MOTOR_STATE = MC_BREAK;
				CPQUser.PressFlag = True;
				TopBlockCause = 1;
			}
		}
	}
	else if(CPQUser.HALLRunCnt >= POS2PRESSCnt)//1350;1250
	{
		if(Foc_lib.ClosedTargetVq > Foc_lib.ClosedPressVq)
		{
			Foc_lib.ClosedTargetVq = Foc_lib.ClosedTargetVq - 30;
			if(Foc_lib.ClosedTargetVq < Foc_lib.ClosedPressVq)
			{
				Foc_lib.ClosedTargetVq = Foc_lib.ClosedPressVq;
			}
		}
		else if(Foc_lib.ClosedTargetVq == Foc_lib.ClosedPressVq)
		{
			CPQUser.PressStableCurrent = Foc_lib.IqFilter;
		}
	}
	else
	{
		if(CPQUser.FilterCnt > 0)CPQUser.FilterCnt--;
	}
}



void CPQPressHallCntScanFun(void)
{//3478-1360= 2118;
	if(CPQUser.HALLRunCnt >= 2200)
	{
		CPQUser.HALLRunCnt = 0;
		CPQ_STATE = RUN_BREAK_TOP;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PressFlag = True;
		if(TopBlockCause == 0)TopBlockCause = 4;
	}
}

void CPQPressScanFun(void)
{
	Foc_lib.TargetIq = CPQUser.PressStableCurrent + CPQUser.PressCurrentLevel;
	CPQPressCurrentScanFun();
//	CPQTopHallBlockScanFun();
}




void CPQBreakBottomScanFun(void)
{
	if(++CPQUser.BREAKRunCnt >= 10)
	{
		CPQUser.BREAKRunCnt = 0;
		Foc_lib.ClosedTargetVq = 0;
		CPQ_STATE = RUN_INIT;
		MOTOR_STATE = MC_STOP;
		CPQUser.HALLRunCnt = 0;//bottom is origin;
		CPQUser.MotorEnable = 0;
		StartValue = 0;
	}
	Hall.HallACnt = 0;
	Hall.HallBCnt = 0;
	Hall.HallCCnt = 0;
}




void CPQBreakTopScanFun(void)
{
	if(CPQ_STATE == RUN_BREAK_TOP)
	{
		if(++CPQUser.BREAKRunCnt >= 100)
		{
			CPQUser.BREAKRunCnt = 0;
			CPQUser.HALLRunCnt = 0;//bottom is origin;
			CPQ_STATE = RUN_STOP;
			MOTOR_STATE = MC_STOP;
			CPQUser.MotorEnable = 0;
			StartValue = 0;
		}
	}
	Hall.HallACnt = 0;
	Hall.HallBCnt = 0;
	Hall.HallCCnt = 0;
}


void CPQBreakIdleScanFun(void)
{
//	static u8 BreakSlope;
	CPQUser.BREAKRunCnt++;
	if(CPQUser.BREAKRunCnt >= 4000)
	{
		CPQUser.BREAKRunCnt = 0;
		CPQ_STATE = RUN_STOP;
		MOTOR_STATE = MC_STOP;
		CPQUser.MotorEnable = 0;
		StartValue = 0;
	}
	else if(CPQUser.BREAKRunCnt >= 1500)
	{
		MOTOR_STATE = MC_BREAK;
		Foc_lib.ClosedTargetVq = 0;
	}
	else
	{
		Foc_lib.ClosedTargetVq = 0;
	}
}


void CPQRUNDirScanFun(void)
{
	if(StartValue == True)
	{
		if(CPQ_STATE == RUN_INIT)
		{
			if(Hall.DirFlag == CCW)
			{
				CPQ_STATE = RUN_POS;
				Foc_lib.ClosedTargetVq = CPQ_RUN_VQ;
			}
			else if(Hall.DirFlag == CW)
			{
				CPQ_STATE = RUN_NEG;
				Foc_lib.ClosedTargetVq = CPQ_RUN_VQ;
			}
		}
	}
}


void RevDirRunFun(void)
{
	if(Hall.DirFlag == CW)
	{
		Hall.AngleSector = CWHALLSectorValue[Hall.NowValue-1];
		Hall.HallAngle = (s16)(HALL_Angle_Val[Hall.AngleSector]);

		HallNextValScanCWFun();
		HallLastValScanCWFun();
	
	}
	else if(Hall.DirFlag == CCW)
	{
		Hall.AngleSector = CCWHALLSectorValue[Hall.NowValue-1];
		Hall.HallAngle = (s16)(HALL_Angle_Val[Hall.AngleSector]);
		Hall.HallAngle = Hall.HallAngle + DEG_180;
		HallNextValScanCCWFun();
		HallLastValScanCCWFun();
	}
}


void CPQDirModifyFun(void)
{
	Foc_lib.Vd = 0;
	Foc_lib.Vq = 0;
	AddStep = ADD_STEP_VAL;
	Hall.HallOmegaPeroid = 0;
	HallCntResetFun();
//	MOTOR_STATE = MC_SQUARE;
}



void CPQPOSRunCurrentScanFun(void)
{
	if(Foc_lib.IqFilter > Foc_lib.PressBusCurrentLevel)
	{
		CPQ_STATE = RUN_BREAK_TOP;
		MOTOR_STATE = MC_BREAK;
		CPQUser.PressFlag = True;
		CPQUser.MotorEnable = 0;
		StartValue = 0;
		if(TopBlockCause == 0)TopBlockCause = 5;
	}
}	
	
void CPQInitStateFun(void)
{
	MOTOR_STATE = MC_STOP;
	CPQUser.HALLRunCnt = 0;//bottom is origin;
	CPQUser.MotorEnable = 0;
	StartValue = 0;	
	Foc_lib.ClosedTargetVq = 0;
}
	
	

void CPQRunSTateFun(void)
{
	switch(CPQ_STATE)
	{
		case RUN_INIT:
		{
//			CPQInitStateFun();
			break;
		}
		case RUN_POS:
		{
			CPQPOSRunFun();
			CPQTopHallBlockScanFun();
			CPQPOSRunCurrentScanFun();
			break;
		}
		case RUN_NEG: 
		{
			CPQNEGRunFun();
			CPQNEGRunCurrentScanFun();
			CPQBottomBlockScanFun();
			break;
		}
		case RUN_PRESS:
		{
			CPQPressScanFun();
			break;
		}			
		case RUN_POUR:
		{
			CPQPourScanFun();
			break;
		}
		case RUN_STOP:
		{
			CPQStopScanFun();
			break;
		}
		case RUN_BREAK_BOTTOM:
		{
			CPQBreakBottomScanFun();
			break;
		}
		case RUN_BREAK_TOP:
		{
			CPQBreakTopScanFun();
			break;
		}
		
		case RUN_BREAK_IDLE:
		{
			CPQBreakIdleScanFun();
			break;
		}
		default:
			break;
	}
}



/**********************end*********************/

 












